import numpy as np
import matplotlib.pyplot as plt
from shapely.geometry import Polygon, Point, LineString, MultiPolygon
from shapely.ops import unary_union, split
import math
from typing import List, Tuple, Optional


class BoustrophedonPartitioner:
    def __init__(self, plot_boundary: List[Tuple[float, float]],
                 obstacles: List[List[Tuple[float, float]]]):
        """
        Boustrophedon分解法分区器

        参数:
        plot_boundary: 地块边界点坐标列表
        obstacles: 障碍物边界点坐标列表
        """
        self.plot_polygon = Polygon(plot_boundary)
        self.obstacles = [Polygon(obstacle) for obstacle in obstacles]

        # 计算工作区域（地块减去障碍物）
        self.work_area = self.plot_polygon
        for obstacle in self.obstacles:
            self.work_area = self.work_area.difference(obstacle)

    def boustrophedon_decomposition(self,
                                    sweep_direction: str = 'vertical',
                                    cell_width: float = 1.0,
                                    ensure_convex: bool = True) -> List[Polygon]:
        """
        Boustrophedon分解法主函数

        参数:
        sweep_direction: 扫描方向 ('vertical' 或 'horizontal')
        cell_width: 扫描线间距/子Cell宽度
        ensure_convex: 是否确保生成凸多边形

        返回:
        子Cell多边形列表
        """
        if sweep_direction == 'vertical':
            return self._vertical_boustrophedon(cell_width, ensure_convex)
        else:
            return self._horizontal_boustrophedon(cell_width, ensure_convex)

    def _vertical_boustrophedon(self, cell_width: float, ensure_convex: bool) -> List[Polygon]:
        """垂直扫描的Boustrophedon分解"""
        minx, miny, maxx, maxy = self.work_area.bounds

        # 生成扫描线位置
        scan_lines = np.arange(minx, maxx + cell_width, cell_width)

        cells = []
        previous_slice = None

        for i in range(len(scan_lines) - 1):
            x1, x2 = scan_lines[i], scan_lines[i + 1]

            # 创建当前扫描带
            current_slice = self._create_vertical_slice(x1, x2, miny, maxy)

            if current_slice and previous_slice is not None:
                # 计算当前切片与前一切片的重叠区域
                overlap = current_slice.intersection(previous_slice)

                if not overlap.is_empty:
                    # 将重叠区域添加到结果中
                    if ensure_convex:
                        convex_parts = self._ensure_convex(overlap)
                        cells.extend(convex_parts)
                    else:
                        if overlap.geom_type == 'MultiPolygon':
                            cells.extend(list(overlap.geoms))
                        else:
                            cells.append(overlap)

            previous_slice = current_slice

        return self._merge_small_cells(cells, min_area=cell_width * cell_width * 0.3)

    def _horizontal_boustrophedon(self, cell_width: float, ensure_convex: bool) -> List[Polygon]:
        """水平扫描的Boustrophedon分解"""
        minx, miny, maxx, maxy = self.work_area.bounds

        # 生成扫描线位置
        scan_lines = np.arange(miny, maxy + cell_width, cell_width)

        cells = []
        previous_slice = None

        for i in range(len(scan_lines) - 1):
            y1, y2 = scan_lines[i], scan_lines[i + 1]

            # 创建当前扫描带
            current_slice = self._create_horizontal_slice(y1, y2, minx, maxx)

            if current_slice and previous_slice is not None:
                # 计算当前切片与前一切片的重叠区域
                overlap = current_slice.intersection(previous_slice)

                if not overlap.is_empty:
                    # 将重叠区域添加到结果中
                    if ensure_convex:
                        convex_parts = self._ensure_convex(overlap)
                        cells.extend(convex_parts)
                    else:
                        if overlap.geom_type == 'MultiPolygon':
                            cells.extend(list(overlap.geoms))
                        else:
                            cells.append(overlap)

            previous_slice = current_slice

        return self._merge_small_cells(cells, min_area=cell_width * cell_width * 0.3)

    def _create_vertical_slice(self, x1: float, x2: float, miny: float, maxy: float) -> Optional[Polygon]:
        """创建垂直切片"""
        slice_polygon = Polygon([
            (x1, miny), (x2, miny), (x2, maxy), (x1, maxy)
        ])

        # 与工作区域求交
        intersection = self.work_area.intersection(slice_polygon)

        if intersection.is_empty:
            return None

        return intersection if intersection.geom_type == 'Polygon' else None

    def _create_horizontal_slice(self, y1: float, y2: float, minx: float, maxx: float) -> Optional[Polygon]:
        """创建水平切片"""
        slice_polygon = Polygon([
            (minx, y1), (maxx, y1), (maxx, y2), (minx, y2)
        ])

        # 与工作区域求交
        intersection = self.work_area.intersection(slice_polygon)

        if intersection.is_empty:
            return None

        return intersection if intersection.geom_type == 'Polygon' else None

    def _ensure_convex(self, polygon: Polygon) -> List[Polygon]:
        """确保多边形为凸多边形"""
        if self._is_convex(polygon):
            return [polygon]

        # 对于非凸多边形，进行三角剖分
        return self._triangulate_polygon(polygon)

    def _is_convex(self, polygon: Polygon) -> bool:
        """检查多边形是否为凸多边形"""
        points = list(polygon.exterior.coords)[:-1]
        n = len(points)

        if n < 3:
            return False

        def cross(o, a, b):
            return (a[0] - o[0]) * (b[1] - o[1]) - (a[1] - o[1]) * (b[0] - o[0])

        signs = set()
        for i in range(n):
            o, a, b = points[i], points[(i + 1) % n], points[(i + 2) % n]
            c = cross(o, a, b)
            if abs(c) > 1e-10:
                signs.add(1 if c > 0 else -1)
                if len(signs) > 1:
                    return False

        return True

    def _triangulate_polygon(self, polygon: Polygon) -> List[Polygon]:
        """简单的多边形三角剖分"""
        points = list(polygon.exterior.coords)[:-1]
        n = len(points)

        if n <= 3:
            return [polygon]

        triangles = []
        remaining_points = points.copy()

        # 简单的耳切法
        while len(remaining_points) > 3:
            n_remaining = len(remaining_points)
            ear_found = False

            for i in range(n_remaining):
                i_prev = (i - 1) % n_remaining
                i_next = (i + 1) % n_remaining

                a, b, c = remaining_points[i_prev], remaining_points[i], remaining_points[i_next]

                # 检查是否为凸顶点
                if self._is_convex_vertex(a, b, c):
                    triangle = Polygon([a, b, c])
                    # 检查是否为有效的耳
                    if self._is_ear(a, b, c, remaining_points):
                        triangles.append(triangle)
                        remaining_points.pop(i)
                        ear_found = True
                        break

            if not ear_found:
                # 如果找不到耳，使用简单方法
                break

        # 添加最后一个三角形
        if len(remaining_points) == 3:
            triangles.append(Polygon(remaining_points))

        return triangles

    def _is_convex_vertex(self, a, b, c) -> bool:
        """检查顶点是否为凸顶点"""
        cross_product = (b[0] - a[0]) * (c[1] - b[1]) - (b[1] - a[1]) * (c[0] - b[0])
        return cross_product > 0

    def _is_ear(self, a, b, c, points) -> bool:
        """检查三角形是否为有效的耳"""
        triangle = Polygon([a, b, c])

        for point in points:
            if point not in [a, b, c]:
                if triangle.contains(Point(point)):
                    return False

        return True

    def _merge_small_cells(self, cells: List[Polygon], min_area: float) -> List[Polygon]:
        """合并小面积的Cell"""
        if not cells:
            return []

        large_cells = []
        small_cells = []

        for cell in cells:
            if cell.area >= min_area:
                large_cells.append(cell)
            else:
                small_cells.append(cell)

        # 尝试将小Cell合并到相邻的大Cell
        merged_cells = large_cells.copy()

        for small_cell in small_cells:
            best_match_idx = -1
            min_distance = float('inf')

            for i, large_cell in enumerate(merged_cells):
                distance = small_cell.distance(large_cell)
                if distance < min_distance and distance < 0.1:
                    min_distance = distance
                    best_match_idx = i

            if best_match_idx >= 0:
                try:
                    union = unary_union([merged_cells[best_match_idx], small_cell])
                    if union.geom_type == 'Polygon':
                        merged_cells[best_match_idx] = union
                    else:
                        merged_cells.append(small_cell)
                except:
                    merged_cells.append(small_cell)
            else:
                merged_cells.append(small_cell)

        return merged_cells

    def optimized_boustrophedon(self,
                                initial_width: float = 2.0,
                                min_cell_area: float = 0.5) -> List[Polygon]:
        """
        优化的Boustrophedon分解

        参数:
        initial_width: 初始扫描宽度
        min_cell_area: 最小Cell面积

        返回:
        优化后的子Cell列表
        """
        # 根据地块形状选择最优扫描方向
        minx, miny, maxx, maxy = self.work_area.bounds
        width = maxx - minx
        height = maxy - miny

        if width >= height:
            sweep_direction = 'vertical'
            optimal_width = max(initial_width, width / 10)  # 至少分成10份
        else:
            sweep_direction = 'horizontal'
            optimal_width = max(initial_width, height / 10)

        print(f"选择{sweep_direction}扫描方向，扫描宽度: {optimal_width:.2f}")

        # 执行分解
        cells = self.boustrophedon_decomposition(
            sweep_direction=sweep_direction,
            cell_width=optimal_width,
            ensure_convex=True
        )

        # 最终优化
        return self._merge_small_cells(cells, min_area=min_cell_area)

    def visualize_decomposition(self,
                                cells: List[Polygon],
                                title: str = "Boustrophedon分解结果",
                                show_sweep_lines: bool = True):
        """可视化分解结果"""
        fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(16, 8))

        # 左图：显示原始地块和障碍物
        self._plot_original_area(ax1)
        ax1.set_title("原始地块和障碍物")

        # 右图：显示分解结果
        self._plot_decomposition_result(ax2, cells, show_sweep_lines)
        ax2.set_title(title)

        plt.tight_layout()
        plt.show()

    def _plot_original_area(self, ax):
        """绘制原始地块"""
        # 绘制地块边界
        x, y = self.plot_polygon.exterior.xy
        ax.plot(x, y, 'k-', linewidth=3, label='地块边界')
        ax.fill(x, y, 'lightblue', alpha=0.3)

        # 绘制障碍物
        for i, obstacle in enumerate(self.obstacles):
            x, y = obstacle.exterior.xy
            ax.fill(x, y, 'red', alpha=0.5, label='障碍物' if i == 0 else "")

        ax.set_aspect('equal')
        ax.legend()
        ax.grid(True, alpha=0.3)

    def _plot_decomposition_result(self, ax, cells: List[Polygon], show_sweep_lines: bool):
        """绘制分解结果"""
        # 绘制子Cell
        colors = plt.cm.tab20(np.linspace(0, 1, len(cells)))

        for i, cell in enumerate(cells):
            if cell.geom_type == 'Polygon':
                x, y = cell.exterior.xy
                ax.fill(x, y, color=colors[i], alpha=0.7)
                ax.plot(x, y, 'blue', linewidth=1)

                # 标注Cell信息
                centroid = cell.centroid
                convex_status = "✓" if self._is_convex(cell) else "✗"
                ax.text(centroid.x, centroid.y,
                        f'{i}\n{cell.area:.2f}\n{convex_status}',
                        ha='center', va='center', fontsize=8,
                        bbox=dict(boxstyle="round,pad=0.3", facecolor="white", alpha=0.8))

        ax.set_aspect('equal')
        ax.grid(True, alpha=0.3)

        # 添加图例说明
        ax.text(0.02, 0.98, "✓: 凸多边形\n✗: 非凸多边形",
                transform=ax.transAxes, fontsize=10,
                bbox=dict(boxstyle="round,pad=0.5", facecolor="yellow", alpha=0.7),
                verticalalignment='top')

    def get_decomposition_info(self, cells: List[Polygon]) -> dict:
        """获取分解统计信息"""
        total_area = self.plot_polygon.area
        obstacle_area = sum(obstacle.area for obstacle in self.obstacles)
        work_area = total_area - obstacle_area
        covered_area = sum(cell.area for cell in cells)

        convex_cells = sum(1 for cell in cells if self._is_convex(cell))

        return {
            'total_cells': len(cells),
            'convex_cells': convex_cells,
            'non_convex_cells': len(cells) - convex_cells,
            'convex_ratio': convex_cells / len(cells) * 100 if cells else 0,
            'total_area': total_area,
            'work_area': work_area,
            'covered_area': covered_area,
            'coverage_rate': covered_area / work_area * 100,
            'average_cell_area': covered_area / len(cells) if cells else 0,
            'min_cell_area': min(cell.area for cell in cells) if cells else 0,
            'max_cell_area': max(cell.area for cell in cells) if cells else 0
        }

    def save_cells_for_path_planning(self, cells: List[Polygon], filename: str):
        """保存子Cell信息供路径规划使用"""
        with open(filename, 'w') as f:
            f.write("# Boustrophedon分解结果 - 全覆盖路径规划数据\n")
            f.write(f"# 总Cell数量: {len(cells)}\n")
            f.write(f"# 工作区域面积: {self.plot_polygon.area - sum(o.area for o in self.obstacles):.2f}\n")
            f.write("# 格式: Cell_ID,凸性,面积,顶点坐标列表\n\n")

            for i, cell in enumerate(cells):
                convex_status = "convex" if self._is_convex(cell) else "nonconvex"
                coords = list(cell.exterior.coords)
                f.write(f"Cell_{i},{convex_status},{cell.area:.4f},{coords}\n")


# 使用示例
def main():
    # 示例数据
    plot_boundary = [(0, 0), (15, 0), (12, 8), (5, 10), (0, 7)]

    obstacles = [
        [(2, 2), (4, 2), (4, 4), (2, 4)],
        [(8, 3), (10, 3), (10, 5), (8, 5)],
        [(6, 6), (8, 6), (8, 8), (6, 8)]
    ]

    # 创建Boustrophedon分区器
    partitioner = BoustrophedonPartitioner(plot_boundary, obstacles)

    print("=== Boustrophedon分解法 ===")

    # 方法1：垂直扫描
    print("\n1. 垂直扫描分解:")
    vertical_cells = partitioner.boustrophedon_decomposition(
        sweep_direction='vertical',
        cell_width=2.0,
        ensure_convex=True
    )
    vertical_info = partitioner.get_decomposition_info(vertical_cells)
    print(f"生成 {vertical_info['total_cells']} 个子Cell")
    print(f"凸多边形比例: {vertical_info['convex_ratio']:.1f}%")
    print(f"覆盖率: {vertical_info['coverage_rate']:.1f}%")

    # 方法2：水平扫描
    print("\n2. 水平扫描分解:")
    horizontal_cells = partitioner.boustrophedon_decomposition(
        sweep_direction='horizontal',
        cell_width=2.0,
        ensure_convex=True
    )
    horizontal_info = partitioner.get_decomposition_info(horizontal_cells)
    print(f"生成 {horizontal_info['total_cells']} 个子Cell")
    print(f"凸多边形比例: {horizontal_info['convex_ratio']:.1f}%")
    print(f"覆盖率: {horizontal_info['coverage_rate']:.1f}%")

    # 方法3：优化分解
    print("\n3. 优化分解:")
    optimized_cells = partitioner.optimized_boustrophedon(
        initial_width=2.0,
        min_cell_area=1.0
    )
    optimized_info = partitioner.get_decomposition_info(optimized_cells)
    print(f"生成 {optimized_info['total_cells']} 个子Cell")
    print(f"凸多边形比例: {optimized_info['convex_ratio']:.1f}%")
    print(f"覆盖率: {optimized_info['coverage_rate']:.1f}%")
    print(f"平均Cell面积: {optimized_info['average_cell_area']:.2f}")

    # 可视化最佳结果
    partitioner.visualize_decomposition(optimized_cells, "优化Boustrophedon分解结果")

    # 保存结果
    partitioner.save_cells_for_path_planning(optimized_cells, "boustrophedon_cells.txt")

    # 打印详细统计
    print("\n=== 详细统计 ===")
    for key, value in optimized_info.items():
        print(f"{key}: {value}")


if __name__ == "__main__":
    main()